#include <stdio.h>
#include <assert.h>

#include <iostream>
#include <algorithm>

#include "attribute.h"
#include "moment.h"

using namespace PBVP;

float GeoAttribute::simScore(const Attribute *attr)
{
	// we use weight 2/3 for the volume and 1/6 each for the
	// lower and upper surface areas
	float volumeSim = 1.0f, lowerAreaSim = 1.0f, upperAreaSim = 1.0f;
	
	const GeoAttribute *geoAttr = (const GeoAttribute *) attr;
		// volume
	float vol1 = this->volume, vol2 = geoAttr->volume;
	if(std::max(vol1, vol2) > 0) {
		volumeSim = std::min(vol1, vol2) / std::max(vol1, vol2);
	}
	// lower surface area
	float area1 = this->lowerArea, area2 = geoAttr->lowerArea;
	if(std::max(area1, area2) > 0) {
		lowerAreaSim = std::min(area1, area2) / std::max(area1, area2);
	}
	// upper surface area
	area1 = this->upperArea, area2 = geoAttr->upperArea;
	if(std::max(area1, area2) > 0) {
		upperAreaSim = std::min(area1, area2) / std::max(area1, area2);
	}

	//return volumeSim;
	return (4.0f*volumeSim + lowerAreaSim + upperAreaSim) / 6.0f;
}

void GeoAttribute::print()
{
	std::cout << "*** DCT node gemotry attributes: innerArea = " << lowerArea << ", outer Area = " << 
		upperArea << ", volume = " << volume << std::endl;
}

std::string GeoAttribute::toString() 
{
    char buf[256];

	sprintf(buf, "*** Geometry attributes: volume = %f, innerArea = %f, outerArea = %f\n", volume,
			lowerArea, upperArea);

	std::string str(buf);
	return str;
}

Attribute* GeoAttribute::clone()
{
	GeoAttribute *geoAttr = new GeoAttribute;
	geoAttr->lowerArea = this->lowerArea;
	geoAttr->upperArea = this->upperArea;
	geoAttr->volume = this->volume;

	return geoAttr;
}

void GeoAttribute::merge(const Attribute* childAttribute, bool inUpperRange)
{
	const GeoAttribute* geoAttr = (const GeoAttribute*) childAttribute;
	this->volume += geoAttr->volume;
	if(inUpperRange) {
		this->upperArea += geoAttr->upperArea;
	} else {
		this->lowerArea += geoAttr->lowerArea;
	}
}

Attribute* GeoAttribute::raise(bool inUpperRange) 
{
	GeoAttribute* geoAttr = new GeoAttribute();
	geoAttr->volume = this->volume;
	if(inUpperRange) {
		geoAttr->upperArea = this->upperArea;
		geoAttr->lowerArea = 0;
	} else {
		geoAttr->lowerArea = this->lowerArea;
		geoAttr->upperArea = 0;
	}
	return geoAttr;
}

float TopoAttribute::simScore(const Attribute* attr)
{
	TopoAttribute* topoAttr = (TopoAttribute *) attr;
	float innerSim = simBetti(innerBetti, topoAttr->innerBetti);
	float outerSim = simBetti(outerBetti, topoAttr->outerBetti);
		
	return 0.5f * (innerSim + outerSim);
}

void TopoAttribute::print()
{
	std::cout << "*** DCT node topology attributes:" << std::endl;
	printf("*** inner Surface Betti Numbers: %d %d %d\n", innerBetti.b0, innerBetti.b1,
			innerBetti.b2);
	printf("*** outer Surface Betti Numbers: %d %d %d\n", outerBetti.b0, outerBetti.b1,
			outerBetti.b2);
}

std::string TopoAttribute::toString() 
{
	char buf[256];
	std::string str;

	sprintf(buf, "*** Toplogy attributes: \n");
	str.append(buf);
	sprintf(buf, "*** inner Surface Betti Numbers: %d %d %d\n", innerBetti.b0, innerBetti.b1,
			innerBetti.b2);
	str.append(buf);
	sprintf(buf, "*** outer Surface Betti Numbers: %d %d %d\n", outerBetti.b0, outerBetti.b1,
			outerBetti.b2);
	str.append(buf);

	return str;
}

Attribute* TopoAttribute::clone()
{
	TopoAttribute* topoAttr = new TopoAttribute;
	
	topoAttr->innerBetti = innerBetti;
	topoAttr->outerBetti = outerBetti;

	return topoAttr;
}

void TopoAttribute::merge(const Attribute *childAttribute, bool inUpperRange)
{
	const TopoAttribute* topoAttr = (const TopoAttribute*)childAttribute;
	if(inUpperRange) {
		this->outerBetti += topoAttr->outerBetti;
		this->outerBetti -= topoAttr->innerBetti;
	} else {
		this->innerBetti += topoAttr->innerBetti;
		this->innerBetti -= topoAttr->outerBetti;
	}
}

Attribute* TopoAttribute::raise(bool inUpperRange)
{
	TopoAttribute *topoAttr = new TopoAttribute;

	topoAttr->outerBetti = this->outerBetti;
	topoAttr->innerBetti = this->innerBetti;
	
	return topoAttr;
}

float ShapeAttribute::simScore(const Attribute* attr)
{
	// compute similary score based on the moment of inertia
	ShapeAttribute* shapeAttr = (ShapeAttribute *) attr;
	Inertia inert1 = calcInertia();
	Inertia inert2 = shapeAttr->calcInertia();
	
	return simInertia(inert1, inert2);
}

void ShapeAttribute::print()
{
	printf("*** DCT shape attributes\n");
	// compute controid
	assert(volume > 0);
	
	Inertia inert = calcInertia();
	
	printf("*** centroid = {%f, %f, %f}\n", inert.centroid[0], inert.centroid[1], inert.centroid[2]);
	printf("*** I1 = %f, I2 = %f, I3 = %f\n", inert.I1, inert.I2, inert.I3);
}

std::string ShapeAttribute::toString() 
{
	char buf[256];
	std::string str;

	Inertia inert = calcInertia();
	sprintf(buf, "*** Shape attributes:\n");
	str.append(buf);
	sprintf(buf, "*** centroid = {%f, %f, %f}\n", inert.centroid[0], inert.centroid[1], inert.centroid[2]);
	str.append(buf);
	sprintf(buf, "*** I1 = %f, I2 = %f, I3 = %f\n", inert.I1, inert.I2, inert.I3);
	str.append(buf);
	return str;
}

Inertia ShapeAttribute::calcInertia()
{
	Inertia inert;

	inert.centroid[0] = x_Integral / volume;
	inert.centroid[1] = y_Integral / volume;
	inert.centroid[2] = z_Integral / volume;
	
	// compute principle inertias
	float **mtx = matrix(1, 3, 1, 3);
	
	//Integral(y^2+z^2)
	mtx[1][1] = y2_Integral - volume*inert.centroid[1]*inert.centroid[1] + 
				z2_Integral - volume*inert.centroid[2]*inert.centroid[2];
	// Integral(x^2+z^2)
	mtx[2][2] = x2_Integral - volume*inert.centroid[0]*inert.centroid[0] +
				z2_Integral - volume*inert.centroid[2]*inert.centroid[2]; 
	// Integral(x^2+y^2)
	mtx[3][3] = x2_Integral - volume*inert.centroid[0]*inert.centroid[0] +
				y2_Integral - volume*inert.centroid[1]*inert.centroid[1];
	// Integral(-xy)
	mtx[1][2] = mtx[2][1] = -(xy_Integral - volume*inert.centroid[0]*inert.centroid[1]);
	// Integral(-xz)
	mtx[1][3] = mtx[3][1] = -(xz_Integral - volume*inert.centroid[0]*inert.centroid[2]);
	// Integral(-yz)
	mtx[2][3] = mtx[3][2] = -(yz_Integral - volume*inert.centroid[1]*inert.centroid[2]);
	
	inert.construct(mtx, volume);

	free_matrix(mtx, 1, 3, 1, 3);
	return inert;
}

Attribute* ShapeAttribute::clone()
{
	ShapeAttribute *shapeAttrib = new ShapeAttribute;

	shapeAttrib->volume = this->volume;
	shapeAttrib->x_Integral = this->x_Integral;
	shapeAttrib->y_Integral = this->y_Integral;
	shapeAttrib->z_Integral = this->z_Integral;
	shapeAttrib->x2_Integral = this->x2_Integral;
	shapeAttrib->y2_Integral = this->y2_Integral;
	shapeAttrib->z2_Integral = this->z2_Integral;
	shapeAttrib->xy_Integral = this->xy_Integral;
	shapeAttrib->xz_Integral = this->xz_Integral;
	shapeAttrib->yz_Integral = this->yz_Integral;

	return shapeAttrib;
}

Attribute* ShapeAttribute::raise(bool inUpperRange)
{
	return clone();
}

void ShapeAttribute::merge(const Attribute *childAttribute, bool inUpperRange)
{
	const ShapeAttribute * shapeAttr = (const ShapeAttribute *)childAttribute;
	volume += shapeAttr->volume;
	x_Integral += shapeAttr->x_Integral;
	y_Integral += shapeAttr->y_Integral;
	z_Integral += shapeAttr->z_Integral;
	x2_Integral += shapeAttr->x2_Integral;
	y2_Integral += shapeAttr->y2_Integral;
	z2_Integral += shapeAttr->z2_Integral;
	xy_Integral += shapeAttr->xy_Integral;
	xz_Integral += shapeAttr->xz_Integral;
	yz_Integral += shapeAttr->yz_Integral;
}

//////////////////////////////////////////////////////////////////////////
float PotentialAttribute::simScore(const Attribute* attr)
{
	const float small = 1e-5f;
	PotentialAttribute* potAttrib = (PotentialAttribute *) attr;
	// normalized potential integral
	float pot1 = this->p_Integral / this->volume;
	float pot2 = potAttrib->p_Integral / potAttrib->volume;
	float sim_p = 1 - fabs(pot1 - pot2) / std::max(small, (float)std::max(fabs(pot1), fabs(pot2)));
	
	// similarity of multipoles
	Multipole pole1 = this->calcMultipole();
	Multipole pole2 = potAttrib->calcMultipole();
	float sim_m = simMultipole(pole1, pole2);

    return 0.81f*sim_p + 0.19f*sim_m;
}

void PotentialAttribute::print()
{
	printf("*** p_Integral = %f, volume = %f\n", p_Integral, volume);
	//printf("px_Integral = %f, py_Integral = %f pz_Integral = %f\n", px_Integral, py_Integral, pz_Integral);
	//printf("px2_Integral = %f, py2_Integral = %f pz2_Integral = %f\n", px2_Integral, py2_Integral, pz2_Integral);
	//printf("pxy_Integral = %f, pxz_Integral = %f pyz_Integral = %f\n", pxy_Integral, pxz_Integral, pyz_Integral);

	Multipole pole = calcMultipole();
	float dipole_len = (float)sqrt(pole.dipole[0]*pole.dipole[0] + pole.dipole[1]*pole.dipole[1] 
								 + pole.dipole[2]*pole.dipole[2]);
	printf("*** dipole length = %f\n", dipole_len);
	printf("*** Q1 = %f Q2 = %f Q3 = %f\n", pole.Q1, pole.Q2, pole.Q3);
}

std::string PotentialAttribute::toString() 
{
	std::string str;
	char buf[256];

	sprintf(buf, "*** Property function attributes: \n");
	str.append(buf);
	sprintf(buf, "*** p_Integral = %f, volume = %f\n", p_Integral, volume);
	str.append(buf);
	Multipole pole = calcMultipole();
	float dipole_len = (float)sqrt(pole.dipole[0]*pole.dipole[0] + pole.dipole[1]*pole.dipole[1] 
								 + pole.dipole[2]*pole.dipole[2]);
	sprintf(buf, "*** dipole length = %f\n", dipole_len);
	str.append(buf);
	sprintf(buf, "*** Q1 = %f Q2 = %f Q3 = %f\n", pole.Q1, pole.Q2, pole.Q3);
	str.append(buf);
	return str;
}

Attribute* PotentialAttribute::clone()
{
	PotentialAttribute* potAttrib = new PotentialAttribute;

	// duplicate from shape attribute. Anyway to remove the duplicates?
	potAttrib->volume = this->volume;
	potAttrib->x_Integral = this->x_Integral;
	potAttrib->y_Integral = this->y_Integral;
	potAttrib->z_Integral = this->z_Integral;

	potAttrib->p_Integral = this->p_Integral;
	potAttrib->px_Integral = this->px_Integral;
	potAttrib->py_Integral = this->py_Integral;
	potAttrib->pz_Integral = this->pz_Integral;
	potAttrib->px2_Integral = this->px2_Integral;
	potAttrib->py2_Integral = this->py2_Integral;
	potAttrib->pz2_Integral = this->pz2_Integral;
	potAttrib->pxy_Integral = this->pxy_Integral;
	potAttrib->pxz_Integral = this->pxz_Integral;
	potAttrib->pyz_Integral = this->pyz_Integral;

	return potAttrib;
}

Attribute* PotentialAttribute::raise(bool inUpperRange)
{
	return clone();
}

void PotentialAttribute::merge(const Attribute *childAttribute, bool inUpperRange)
{
	const PotentialAttribute* potAttrib = (const PotentialAttribute*) childAttribute;
	
	volume += potAttrib->volume;
	x_Integral += potAttrib->x_Integral;
	y_Integral += potAttrib->y_Integral;
	z_Integral += potAttrib->z_Integral;

	p_Integral += potAttrib->p_Integral;
	px_Integral += potAttrib->px_Integral;
	py_Integral += potAttrib->py_Integral;
	pz_Integral += potAttrib->pz_Integral;
	px2_Integral += potAttrib->px2_Integral;
	py2_Integral += potAttrib->py2_Integral;
	pz2_Integral += potAttrib->pz2_Integral;
	pxy_Integral += potAttrib->pxy_Integral;
	pxz_Integral += potAttrib->pxz_Integral;
	pyz_Integral += potAttrib->pyz_Integral;
}

Multipole PotentialAttribute::calcMultipole()
{
	Multipole pole;
	
	assert(volume > 0);

	pole.centroid[0] = x_Integral / volume;
	pole.centroid[1] = y_Integral / volume;
	pole.centroid[2] = z_Integral / volume;
	
	// compute the dipole
	pole.dipole[0] = (px_Integral - p_Integral * pole.centroid[0]) / volume;
	pole.dipole[1] = (py_Integral - p_Integral * pole.centroid[1]) / volume;
	pole.dipole[2] = (pz_Integral - p_Integral * pole.centroid[2]) / volume;

	// compute principal values of the qudrupole
	float **mtx = matrix(1, 3, 1, 3);
	// substract centroid from the integral
	float px2 = px2_Integral + p_Integral*pole.centroid[0]*pole.centroid[0] - 2*pole.centroid[0]*px_Integral;
	float py2 = py2_Integral + p_Integral*pole.centroid[1]*pole.centroid[1] - 2*pole.centroid[1]*py_Integral;
	float pz2 = pz2_Integral + p_Integral*pole.centroid[2]*pole.centroid[2] - 2*pole.centroid[2]*pz_Integral;
	float pr2 = px2 + py2 + pz2;
	mtx[1][1] = 3*px2 - pr2;
	mtx[2][2] = 3*py2 - pr2;
	mtx[3][3] = 3*pz2 - pr2;
	mtx[1][2] = mtx[2][1] = 3*(pxy_Integral + p_Integral*pole.centroid[0]*pole.centroid[1] -
							   px_Integral*pole.centroid[1] - py_Integral*pole.centroid[0]);
	mtx[1][3] = mtx[3][1] = 3*(pxz_Integral + p_Integral*pole.centroid[0]*pole.centroid[2] -
							   px_Integral*pole.centroid[2] - pz_Integral*pole.centroid[0]);
	mtx[2][3] = mtx[3][2] = 3*(pyz_Integral + p_Integral*pole.centroid[1]*pole.centroid[2] -
							   py_Integral*pole.centroid[2] - pz_Integral*pole.centroid[1]);

	float *eigenVal = fvector(1, 3);
	float **eigenVec = matrix(1, 3, 1, 3);
	int nrot;
	// compute eigen values
	jacobi(mtx, 3, eigenVal, eigenVec, &nrot);
	// sort eigen values
	eigsrt(eigenVal, eigenVec, 3);
		
	pole.Q1 = eigenVal[1] / volume;
	pole.Q2 = eigenVal[2] / volume;
	pole.Q3 = eigenVal[3] / volume; 
		
	free_vector(eigenVal, 1, 3);
	free_matrix(eigenVec, 1, 3, 1, 3);
	free_matrix(mtx, 1, 3, 1, 3);
	return pole;
}